#ifndef MY_INC_PHY_PHY_TOGI_H_
#define MY_INC_PHY_PHY_TOGI_H_

#include <stdint.h>
#include <stdio.h>

/*********变换参数定义****************/
#define TOGI_ALIGNMENT_MODE        1U // 选择模式为1

#define TOGI_U_ZERO_EN             1U              // 启用零序电压计算
#define TOGI_U_ZERO_DIS            0U              // 禁用零序电压计算
#define TOGI_U_ZERO_MODE           TOGI_U_ZERO_DIS // 启用零序电压计算

#define TOGI_THETA_ADJUSTED(THETA) (TOGI_ALIGNMENT_MODE) ? THETA - _PI_D2 : THETA // 选择模式为1 ，滞后90度

/*
 * 宏名：_TOGI_GET_SIN_COS
 * 功能：获取SIN和COS值
 * 参数：
 * 		THETA：当前角度，float类型
 * 		Sin_Value：指向存储SIN值的变量的指针
 * 		Cos_Value：指向存储COS值的变量的指针
 * 返回值：无
 */
typedef struct
{
    float TOGI_k; // TOGI增益
    float TOGI_T; // TOGI时间常数

    float integral1_Yn;   // 积分器1
    float integral1_Xn;   // 积分器1
    float integral1_Xn_1; // 积分器1

    float integral2_Yn;   // 积分器1
    float integral2_Xn;   // 积分器1
    float integral2_Xn_1; // 积分器1

    float integral3_Yn;   // 积分器1
    float integral3_Xn;   // 积分器1
    float integral3_Xn_1; // 积分器1

    float U1; // 输出1
    float U2; // 输出2
    float U3; // 输出3

    float Alpha; // Alpha电压
    float Beta;  // Beta电压

    float D; // D电压
    float Q; // Q电压
} TOGI_Typedef;
// 函数声明：TOGI初始化
void TOGI_Init(TOGI_Typedef *transform, float TOGI_k, float T_s);
void TOGI_Reset(TOGI_Typedef *transform);

/******变换函数**********/
// 函数声明：TOGI到Alpha_Beta_Zero变换
// 执行时间：74.28ns
void               TOGI_to_Alpha_Beta_Zero(TOGI_Typedef *transform, float Input, float W);
static inline void TOGI_to_Alpha_Beta_Zero_Inline(TOGI_Typedef *transform, float Input, float W)
{
    float temp = (Input - transform->U1) * transform->TOGI_k;
    transform->integral1_Xn = (temp - transform->U3) * W;
    transform->U3 = transform->integral1_Yn =
        transform->integral1_Yn + transform->TOGI_T * (transform->integral1_Xn + transform->integral1_Xn_1); // 积分
    transform->integral1_Xn_1 = transform->integral1_Xn;                                                     // 移位

    transform->integral2_Xn = (temp - transform->U2) * W;
    transform->U1 = transform->integral2_Yn =
        transform->integral2_Yn + transform->TOGI_T * (transform->integral2_Xn + transform->integral2_Xn_1); // 积分
    transform->integral2_Xn_1 = transform->integral2_Xn;                                                     // 移位

    transform->integral3_Xn = (transform->U1 * W);
    transform->U2 = transform->integral3_Yn =
        transform->integral3_Yn + transform->TOGI_T * (transform->integral3_Xn + transform->integral3_Xn_1); // 积分
    transform->integral3_Xn_1 = transform->integral3_Xn;                                                     // 移位
    transform->Alpha = transform->U1;
    transform->Beta = transform->U2 - transform->U3;
}

void               TOGI_to_DQ0(TOGI_Typedef *transform, float Input, float W, float *Sin_Data, float *Cos_Data);
static inline void TOGI_TO_DQ0_Inline(TOGI_Typedef *transform, float Input, float W, float *Sin_Data, float *Cos_Data)
{

    TOGI_to_Alpha_Beta_Zero_Inline(transform, Input, W);
    // DQ0变换
    transform->D = transform->Alpha * (*Sin_Data) - transform->Beta * (*Cos_Data);
    transform->Q = transform->Alpha * (*Cos_Data) + transform->Beta * (*Sin_Data);
}
// 函数声明：DQ0到TOGI变换
void DQ0_to_TOGI(TOGI_Typedef *transform, float *Sin_Data, float *Cos_Data);

// 宏定义：DQ0到TOGI变换
static inline void DQ0_TO_TOGI_Inline(TOGI_Typedef *transform, float *Sin_Data, float *Cos_Data)
{
    transform->Alpha = transform->D * (*Cos_Data) - transform->Q * (*Sin_Data);
}

typedef struct
{
    TOGI_Typedef TOGI1;
    TOGI_Typedef TOGI2;

    float Alpha; // Alpha电压
    float Beta;  // Beta电压

    float D;    // D电压
    float Q;    // Q电压
    float Zero; // 0电压
} ThreePhaseTOGI_Typedef;

static inline void TOGI_ABC_to_DQ0_Inline(ThreePhaseTOGI_Typedef *transform, float Input[3], float W)
{
    transform->Zero = 0.333333333f * (Input[0] + Input[1] + Input[2]);
    transform->Alpha = (0.66666667f * Input[0]) - (0.333333333f * Input[1]) - (0.333333333f * Input[2]); // 计算α分量
    transform->Beta = 0.577350269f * (Input[2]);                                                         // 计算β分量
    TOGI_to_Alpha_Beta_Zero_Inline(&transform->TOGI1, transform->Alpha, W);
    TOGI_to_Alpha_Beta_Zero_Inline(&transform->TOGI2, transform->Beta, W);
    transform->D = 0.5f * (transform->TOGI1.Alpha + transform->TOGI2.Beta);
    transform->Q = 0.5f * (transform->TOGI1.Beta - transform->TOGI2.Alpha);
}

#endif /* INC_COMPENSATION_H_ */
